package JET.physics;

import JET.geom.Vec2d;
import JET.physics.bodies.SpringBody;
import java.util.Collection;

/**
 *
 * @author 057Ry
 */
public class PhysicsHelper
{
    public static Vec2d calcSpringForce(Vec2d dir, double startLength, Vec2d velA, Vec2d velB,
                          double length, double ks, double kd)
    {
        double dist = length - startLength;

        //Vector2 relVel = velA - velB;
        Vec2d relVel = new Vec2d(velB);
        relVel.sub(velA);

        double totalRelVel = relVel.dot(dir);

        //return dir * ((dist * springK) - (totalRelVel * damping));
        return dir.getScaled( ((dist*ks) - (totalRelVel*kd)) );
    }

    public static Vec2d calcShapeMachingForce(Vec2d a, Vec2d b, Vec2d vel, double ks, double kd)
    {
        Vec2d dir = b.getSubbed(a);

        double dist = dir.getMagnitude();
        if (dist > 0.001)
            dir.normalize();
        else
            dir.setXY(0., 0.);

        double dotVel = dir.dot(vel);

        double fVal = dist*ks - dotVel*kd;
        return dir.getScaled(fVal);
    }

    public static Vec2d getGeomCenterOfMasses(Collection<Mass> mList) {
        Vec2d center = new Vec2d();
        for(Mass m : mList)
            center.add(m.getPosition());
        center.scale(1./mList.size());
        return center;
    }

    public static Vec2d getGeomCenterOfVecs(Collection<Vec2d> vList) {
        Vec2d center = new Vec2d();
        for(Vec2d v : vList)
            center.add(v);
        center.scale(1./vList.size());
        return center;
    }

    public static Vec2d getMassCenterOfMasses(Collection<Mass> mList) {
        Vec2d center = new Vec2d();
        double sum = 0.;
        for(Mass m : mList) {
            center.add(m.getPosition().getScaled(m.getMass()));
            sum += m.getMass();
        }
        center.scale(1./sum);
        return center;
    }

    public static double getMomentOfInteria(Collection<Mass> mList) {
        Vec2d mCenter = getMassCenterOfMasses(mList);
        double sum = 0.;
        for(Mass m : mList) {
            Vec2d r = m.getPosition().getSubbed(mCenter);
            sum += m.getMass()*r.getMagnitudeSquared();
        }
        return sum;
    }

    public static double getMomentOfInteria(Collection<Vec2d> vList, double mass) {
        Vec2d gCenter = getGeomCenterOfVecs(vList);
        mass /= vList.size();
        double sum = 0.;
        for(Vec2d v : vList) {
            Vec2d r = v.getSubbed(gCenter);
            sum += mass*r.getMagnitudeSquared();
        }
        return sum;
    }

    public static double getMass(SpringBody body) {
        double mass = .0;
        Collection<Mass> massCol = body.getMassListSync();
        synchronized(massCol) {
            for(Mass m : massCol)
                mass += m.getMass();
        }
        return mass;
    }

    public static Vec2d getGeomCenter(SpringBody body) {
        Vec2d v = new Vec2d();
        Collection<Mass> massCol = body.getMassListSync();
        synchronized(massCol) {
            for(Mass m : massCol)
                v.add(m.getPosition());
            v.scale(1./massCol.size());
        }
        return v;
    }

    public static Vec2d getVelocity(SpringBody body) {
        Vec2d v = new Vec2d();
        Collection<Mass> massCol = body.getMassListSync();
        synchronized(massCol) {
            for(Mass m : massCol)
                v.add(m.getVelocity());
            v.scale(1./massCol.size());
        }
        return v;
    }

    public static Vec2d getForce(SpringBody body) {
        Vec2d v = new Vec2d();
        Collection<Mass> massCol = body.getMassListSync();
        synchronized(massCol) {
            for(Mass m : massCol)
                v.add(m.getForce());
            v.scale(1./massCol.size());
        }
        return v;
    }

    public static double getKineticEnergy(SpringBody sb)
    {
        return getMass(sb) * getVelocity(sb).getMagnitudeSquared() / 2.;
    }

    public static double getPotentialEnergy(SpringBody sb, double g)
    {
        return getMass(sb) * g * getGeomCenter(sb).getY();
    }

    public static double getPotentialEnergy(SpringBody sb, double g, double groundLevel)
    {
        return getMass(sb) * g * (getGeomCenter(sb).getY()-groundLevel);
    }
}
